#include <ESP32Servo.h>
#include <MPU6050_tockn.h>
#include <Wire.h>
#include "MyUDP.h"

MPU6050 mpu6050(Wire);
MyUDP myUDP;

Servo motor1;
Servo motor2;
Servo motor3;
Servo motor4;

const int BASIC_SPEED=1000;

float balanceX=4.4, balanceY=-3.0, balanceZ=0;
float xp = 1.2, xi =0.01, xd =0.30;
float yp = 1.0, yi =0.01, yd =0.20;

float angleX,angleY,gyroX,gyroY;
float biasX,biasY;
float integrateX,integrateY;
int pwm=0,pwmXPID=0,pwmYPID=0,pwmZPID=0;

// 定义四个电机的当前PWM值
int motor1Speed = 1000;
int motor2Speed = 1000;
int motor3Speed = 1000;
int motor4Speed = 1000;

int lauched=0;
String info="";
float gas=0.0;
bool check=false;
int pingNum=0,pongNum=0;

// int xOutterLoop(int expAngleX){
//   biasX = mpu6050.getAngleX()-expAngleX;
//   integralX += biasX;
//   integralX = constrain(integralX, -800, 800);
//   return xp * biasX + xi * integralX + xd * mpu6050.getGyroX();
// }

// int xInnerLoop(int outX){
//   errX = outX - mpu6050.getGyroX();
//   integralGX += errX;
//   integralGX = constrain(integralGX, -800, 800);
//   int pitchData = xp2 * errX + xi2 * integralGX + xd2 * (errX - lastGXErr);
//   lastGXErr = errX;
//   return pitchData;
// }

void speed(Servo *theServo,int num){
  num = constrain(num, 1000, 2000);
  theServo->writeMicroseconds(num);
}

void softLauch(Servo *theServo){
  for (int i=840; i<1190; i++){
    speed(theServo,i);
    delay(10);
  }
  speed(theServo,1000);
}

void startMotors(){
  myUDP.sendData("startMotors");

  pinMode(26, OUTPUT);
  pinMode(27, OUTPUT);
  pinMode(32, OUTPUT);
  pinMode(33, OUTPUT);
  motor1.attach(26);
  motor2.attach(27);
  motor3.attach(32);
  motor4.attach(33);
  //
  softLauch(&motor1);
  softLauch(&motor2);
  softLauch(&motor3);
  softLauch(&motor4);
}

void stopMotors(){
  gas=0;
  speed(&motor1,1000);
  speed(&motor2,1000);
  speed(&motor3,1000);
  speed(&motor4,1000);
}

/* 

   head
1       2
 \     /
  \   /
  /   \
 /     \
3       4

motor1: pwm1 = pwm + pwmXPID + pwmYPID + pwmZPID;
motor2: pwm2 = pwm + pwmXPID - pwmYPID - pwmZPID;
motor3: pwm3 = pwm - pwmXPID + pwmYPID - pwmZPID;
motor4: pwm4 = pwm - pwmXPID - pwmYPID + pwmZPID;

*/
void flyBalance(){
  mpu6050.update();
  angleX=mpu6050.getAngleX();
  angleY=mpu6050.getAngleY();
  gyroX=mpu6050.getGyroX();
  gyroY=mpu6050.getGyroY();

  //Serial.println("angleX:"+String(angleX));
  //Serial.println("angleY:"+String(angleY));

  if(gas==0){
    stopMotors();
    return;
  }

  biasX = angleX - balanceX;
  biasY = angleY - balanceY;

  integrateX += biasX;
  integrateY += biasY;

  integrateX=constrain(integrateX, -800, 800);
  integrateY=constrain(integrateY, -800, 800);

  pwmXPID=xp * biasX + xi * integrateX + xd * gyroX;
  pwmYPID=yp * biasY + yi * integrateY + yd * gyroY;

  pwmXPID = pwmXPID / 2;
  pwmYPID = pwmYPID / 2;
  pwmZPID = pwmZPID / 2;
  //pwmXPID = 0;
  //pwmYPID = 0;
  pwmZPID = 0;

  pwmXPID=constrain(pwmXPID, -500, 500);
  pwmYPID=constrain(pwmYPID, -500, 500);

  pwm = BASIC_SPEED * ( 1 + gas );

  motor1Speed = pwm + pwmXPID + pwmYPID + pwmZPID;
  motor2Speed = pwm + pwmXPID - pwmYPID - pwmZPID;
  motor3Speed = pwm - pwmXPID + pwmYPID - pwmZPID;
  motor4Speed = pwm - pwmXPID - pwmYPID + pwmZPID;

  speed(&motor1,motor1Speed);
  speed(&motor2,motor2Speed);
  speed(&motor3,motor3Speed);
  speed(&motor4,motor4Speed);
}

void changeSpeed(const char *s){
  switch (*s){
    case 'F':
      gas += 0.1;
      if(gas>1.0){
        gas = 1.0;
      }
      break;
    case 'B':
      gas -= 0.1;
      if(gas<0){
        gas=0;
      }
      break;
    case 'L':
      gas=0.5;
      break;
    case 'R':
      gas=0;
      break;

    case '0':
      stopMotors();
      balanceX=0.2;
      balanceY=0.7;
      balanceZ=0;
      break;
    case '1':
      if(lauched==0){
        lauched = 1;
        startMotors();
      }else{
        check=true;
      }
      break;

    case '2':
      xp+=0.1;
      //yp+=0.1;
      break;
    case '3':
      xp-=0.1;
      //yp-=0.1;
      break;
    case '4':
      xi+=0.01;
      //yi+=0.01;
      break;
    case '5':
      xi-=0.01;
      //yi-=0.01;
      break;
    case '6':
      xd+=0.01;
      //yd+=0.01;
      break;
    case '7':
      xd-=0.01;
      //yd-=0.01;
      break;
    case 'c':
      balanceX +=10;
      //balanceY +=10;
      break;
    case 'd':
      balanceX -=10;
      //balanceY -=10;
      break;
  }
  gas = constrain(gas, 0.0, 1.0);
  myUDP.sendData("gas:"+String(gas)+" xp:"+String(xp)+" xi:"+String(xi)+" xd:"+String(xd)+" balanceX:"+String(balanceX));
  //myUDP.sendData("gas:"+String(gas)+" yp:"+String(yp)+" yi:"+String(yi)+" yd:"+String(yd)+" balanceY:"+String(balanceY));
}

void sendPing(){
  pingNum++;
  if(pingNum>=500){
    pingNum=0;
    myUDP.sendData("@ping@");
  }
}

void checkPong(String data){
  pongNum++;
  if(data=="@pong@"){
    pongNum=0;
    Serial.println("pong!");
  }else{
    if(pongNum>2000){
      pongNum=0;
      Serial.println("=======no pong!========");
      stopMotors();
    }
  }
}

void resetError(){
  pinMode(26, OUTPUT);
  pinMode(27, OUTPUT);
  pinMode(32, OUTPUT);
  pinMode(33, OUTPUT);
  digitalWrite(26, 0);
  digitalWrite(27, 0);
  digitalWrite(32, 0);
  digitalWrite(33, 0);
}

void setup() {
  Serial.begin(9600);

  resetError();

  myUDP=MyUDP();
  myUDP.createWiFi("ESP32_WIFI","12345678", 3001);

  Wire.begin(21,22);
  mpu6050.begin();
  //mpu6050.calcGyroOffsets(true);
  mpu6050.setGyroOffsets(0.28,-0.50,0.30);
}

void loop() {
  flyBalance();

  info=myUDP.getData();
  if(info!="" && info!="@pong@"){
    changeSpeed(info.c_str());
  }

  if(gas>0){
    sendPing();
    checkPong(info);
  }

  if(check){
    check=false;
    myUDP.sendData("X:"+String(angleX)+" Y:"+String(angleY));
    angleX = angleX-balanceX;
    angleY = angleY-balanceY;
    myUDP.sendData("X2:"+String(angleX)+" Y2:"+String(angleY));
  }
}





